#include "page.h"
#include "imx6ul.h"

#include "asm/pgtable.h"

void cpu_armv7_set_pte(pte_t *pte_table, unsigned long entry)
{
    entry &= ~(0xfff);
    
    //                       nG           s          apx      TEX[8:6]    AP[5:4]     C           B         small      Xn(exec)              
    pte_table->pte = entry | (1 << 11) | (0 << 10) | (0 << 9) | (0 << 6) | (3 << 4) | (1 << 3) | (1 << 2) | (1 << 1) | (1 << 0);
}

void cpu_armv7_set_pmd(pmd_t *pmd, unsigned long entry)
{
    pmd->pmd = entry | (3 << 5) | 0x01;
}

void cpu_armv7_set_pgd(unsigned long entry)
{
    uint32_t L1Base = (uint32_t)entry;

    /* Use TTBR translation, with 16KB L1Table size (N=0) */
    __set_TTBCR(0);

    /* Set TTBR0 with inner/outer write back write allocate and not shareable, [4:3]=01, [1]=0, [6,0]=01 */
    __set_TTBR0((L1Base & 0xFFFFC000UL) | 0x9UL);

    /* Set all domains to client */
    __set_DACR(0x55555555UL);

    /* Set PROCID and ASID to 0 */
    __MCR(15, 0, 0, 13, 0, 1);
}

int arch_early_map_addr(unsigned long vaddr, unsigned long size, unsigned long paddr, pgprot_t prot)
{
    unsigned long vend = vaddr + size - 1;
    mmu_attribute_t s_mmuDevAttr = {.type = MMU_MemoryDevice,
                                    .domain = 0U,
                                    .accessPerm = MMU_AccessRWNA,
                                    .shareable = 0U,
                                    .notSecure = 0U,
                                    .notGlob = 0U,
                                    .notExec = 1U};

    paddr &= 0xfff00000;
    do
    {
        MMU_ConfigSection((uint32_t *)swapper_pg_dir, (const void *)vaddr, paddr, &s_mmuDevAttr);
        vaddr += 0x100000;
        paddr += 0x100000;
    } while (vaddr < vend);

    return 0;
}